#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import rospy
import actionlib

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal


class Nav():
    def __init__(self):
        self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)

        # 起始位置
        self.goal_standby = MoveBaseGoal()
        self.goal_standby.target_pose.header.frame_id = 'map'
        self.goal_standby.target_pose.pose.position.x = 0.125
        self.goal_standby.target_pose.pose.position.y = -0.737
        self.goal_standby.target_pose.pose.position.z = 0.000
        self.goal_standby.target_pose.pose.orientation.x = 0.000
        self.goal_standby.target_pose.pose.orientation.y = 0.000
        self.goal_standby.target_pose.pose.orientation.z = 0.468
        self.goal_standby.target_pose.pose.orientation.w = 0.884

        # 初始化抓取垃圾的位置
        self.goal_pick = MoveBaseGoal()
        self.goal_pick.target_pose.header.frame_id = 'map'
        self.goal_pick.target_pose.pose.position.x = 0.079
        self.goal_pick.target_pose.pose.position.y = 0.955
        self.goal_pick.target_pose.pose.position.z = 0.00
        self.goal_pick.target_pose.pose.orientation.x = 0.00
        self.goal_pick.target_pose.pose.orientation.y = 0.00
        self.goal_pick.target_pose.pose.orientation.z = 0.702
        self.goal_pick.target_pose.pose.orientation.w = 0.712

        # 初始化放置垃圾的位置
        self.goal_place = MoveBaseGoal()
        self.goal_place.target_pose.header.frame_id = 'map'
        self.goal_place.target_pose.pose.position.x = 1.110
        self.goal_place.target_pose.pose.position.y = -0.670
        self.goal_place.target_pose.pose.position.z = 0.000
        self.goal_place.target_pose.pose.orientation.x = 0.000
        self.goal_place.target_pose.pose.orientation.y = 0.000
        self.goal_place.target_pose.pose.orientation.z = -0.700
        self.goal_place.target_pose.pose.orientation.w = 0.700

    def active_callback(self):
        """
        action激活后回调（一次）
        """
        rospy.logdebug('joint trajectory action is actived')

    def done_callback(self, state, res):
        """
        action完成后回调（一次）
        """
        rospy.logdebug('joint trajectory action is finished')
        rospy.logdebug('state: %s', state)
        rospy.logdebug('res: %s', res)

    def feedback_callback(self, feedback):
        """
        action执行过程中反馈的回调（多次）
        """
        rospy.logdebug('joint trajectory action is feedbacking...')
        rospy.logdebug('feedback: %s', feedback)

    def move_base_send_goal(self, goal):
        """
        action请求
        """
        self.move_base_client.wait_for_server()

        self.move_base_client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.move_base_client.wait_for_result(rospy.Duration.from_sec(300.0))

    def nav_pick(self):
        """
        小车导航到垃圾区
        """
        rospy.loginfo('nav pick...')

        self.move_base_send_goal(self.goal_pick)

        rospy.loginfo('nav pick OK')

    def nav_place(self):
        """
        小车导航到垃圾区放置区
        """
        rospy.loginfo('nav place...')

        self.move_base_send_goal(self.goal_place)

        rospy.loginfo('nav place OK')

    def nav_stanby(self):
        """
        小车导航到起始位置
        """
        rospy.loginfo('nav stanby...')

        self.move_base_send_goal(self.goal_standby)

        rospy.loginfo('nav stanby OK')


if __name__ == '__main__':
    rospy.init_node('npu_robot_nav', anonymous=True, log_level=envs.log_level)
    nav = Nav()
    nav.nav_pick()
    nav.nav_place()
    nav.nav_stanby()
